本篇文章探討moveit的運動學，moveit提供的API可以從URDF檔中解手臂運動學。以Moveit範例連結做示範。
moveit使用RobotModel與RobotState這兩個class做運動學相關的計算。RobotModel負責從URDF、SRDF
中抓取該手臂的幾何參數，比如:桿件參數、關節參數..等等，RobotState則是可以進行運動學上的操作。

首先此範例在launch中載入上述需要的參數。首先執行robot_model_and_robot_state_tutorial.launch，這當中還執行一個planning_context.launch，planning_context內將檔案panda_arm.urdf.xacro讀取到robot_description內。panda_moveit_config)/config/panda_arm.srdf.xacro放入robot_description_semantic內。
其他參數:robot_description_planning、robot_description_kinematics也都放入相對應的檔案joint_limits.yaml、kinematics.yaml。

程式碼部分這參考連結一中將程式碼加上註解，可以參考此說明。
第一段程式碼:

robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str());

robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));
kinematic_state->setToDefaultValues();
    
const robot_state::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("panda_arm");
const std::vector<std::string>& joint_names = joint_model_group->getVariableNames();

第一行的robot_description，就是檔案panda_arm.urdf.xacro，執行程式時可以在另一個視窗中使用rosparam list、rosparam get /robot_description 查詢內容。

這一段使用RobotModelLoader作資料讀取。使用RobotModelLoader是因為，RobotState宣告時可以放入自己跟RobotModel。至於程式碼中的RobotStatePtr、RobotModelPtr，經過測試，將指令替換成RobotState、RobotModel，結果是一樣的，只是Ptr是指標類型。robot_State實作連結

std::vector<double> joint_values;
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
for (std::size_t i = 0; i < joint_names.size(); ++i)
{
  ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
}

這一段單純使用copyJointGroupPositions指令取出個關節角度並且printf出來。copyJointGroupPositions的用法可以見RobotState的連結，copyJointGroupPositions有很多的多載，可以取出一個角度、全部的角度。

joint_values[0] = 5.57;
kinematic_state->setJointGroupPositions(joint_model_group, joint_values);
ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));
kinematic_state->enforceBounds();
ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));

測試角度範圍，satisfiesBounds會回傳true、false來判斷。這角度設定的資料儲存在檔案panda_arm_xacro內，也就是被launch所呼叫的檔案。
使用enforceBounds之後，會把超過的角度修正到Limit內，以這題為例joint_values設為5.57，經過enforceBounds之後，把角度印出來看，joint_values[0]會變成2.89，2.89是panda_arm_xacro內的Limit設定參數)。

以上內容屬於熱身，下一段進入正題正反向運動學。

kinematic_state->setToRandomPositions(joint_model_group);
const Eigen::Affine3d& end_effector_state = kinematic_state->getGlobalLinkTransform("panda_link8");

ROS_INFO_STREAM("Translation: \n" << end_effector_state.translation() << "\n");
ROS_INFO_STREAM("Rotation: \n" << end_effector_state.rotation() << "\n");

這部分使用setToRandomPositions隨機產生角度，自己測試時可以採用上一段的setJointGroupPositions自行給數值去代替。getGlobalLinkTransform是取出該link的資料，此範例用的是panda_link8，也可以換成其他link。

下一段反向運動學:

std::size_t attempts = 10;
double timeout = 0.1;
bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, attempts, timeout);

if (found_ik)
{
  kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
  for (std::size_t i = 0; i < joint_names.size(); ++i)
  {
    ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
  }
}
else
{
  ROS_INFO("Did not find IK solution");
}

由於這運動學是使用數值解，一開始要先設定attempts也就是迭代次數，至於最佳的迭代次數還要進一步測試。
這範例中使用setFromIK，放入上一節正向運動學的end_effector_state，可以自行宣告一組Affine3d去定義想要的座標。

Eigen::Vector3d reference_point_position(0.0, 0.0, 0.0);
Eigen::MatrixXd jacobian;
kinematic_state->getJacobian(joint_model_group,
                             kinematic_state->getLinkModel(joint_model_group->getLinkModelNames().back()),
                             reference_point_position, jacobian);
ROS_INFO_STREAM("Jacobian: \n" << jacobian << "\n");

最後一段是運動學中較困難的Jacobian。範例中的Jacobian為6x8 因為該手臂有8軸。第一個row到第三個row是速度，第四個row到最後是角速度。
不過這範例中的結果來分析，這裡的Jacobian是沒有乘上角速度，所以第四個row到第六個row其實是軸方向。所以如果要後續的計算，如:速度、角速度、力量分析，就要利用這結果去算